#ifndef EGO_PLANNER_ADAPTER__TARGET_RELAY_HPP_
#define EGO_PLANNER_ADAPTER__TARGET_RELAY_HPP_

#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <std_msgs/msg/float64.hpp>
#include <std_msgs/msg/bool.hpp>
#include <visualization_msgs/msg/marker.hpp>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <memory>

namespace ego_planner_adapter {

/**
 * @brief Target relay node for converting RViz goals to ego-planner format
 * 
 * This node:
 * - Receives 2D Nav Goals from RViz
 * - Adds altitude information from a separate topic
 * - Converts to 3D pose and forwards to ego-planner
 * - Provides visualization of 3D targets
 */
class TargetRelay : public rclcpp::Node {
public:
    explicit TargetRelay(const rclcpp::NodeOptions& options = rclcpp::NodeOptions());

private:
    // Callbacks
    void rvizGoalCallback(const std::shared_ptr<const geometry_msgs::msg::PoseStamped>& msg);
    void targetAltitudeCallback(const std::shared_ptr<const std_msgs::msg::Float64>& msg);
    void triggerCallback(const std::shared_ptr<const std_msgs::msg::Bool>& msg);
    
    // Processing functions
    void processGoal(const geometry_msgs::msg::PoseStamped& goal_2d);
    void publishGoalVisualization(const geometry_msgs::msg::PoseStamped& goal_3d);
    bool transformPose(const geometry_msgs::msg::PoseStamped& input_pose,
                      geometry_msgs::msg::PoseStamped& output_pose,
                      const std::string& target_frame);
    
    // Subscribers
    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr rviz_goal_sub_;
    rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr target_altitude_sub_;
    rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr trigger_sub_;
    
    // Publishers
    rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr ego_goal_pub_;
    rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr goal_viz_pub_;
    rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr planning_trigger_pub_;
    
    // TF2
    std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
    std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
    
    // State
    double target_altitude_;
    bool has_altitude_;
    geometry_msgs::msg::PoseStamped pending_goal_;
    bool has_pending_goal_;
    
    // Parameters
    std::string map_frame_;
    std::string base_frame_;
    double default_altitude_;
    double goal_tolerance_;
    bool auto_trigger_;
    double marker_scale_;
    
    // Goal management
    rclcpp::Time last_goal_time_;
    geometry_msgs::msg::PoseStamped current_goal_;
    bool has_current_goal_;
    
    // Markers
    int marker_id_;
    
    // Constants
    static constexpr double GOAL_TIMEOUT = 30.0; // seconds
    static constexpr double MIN_GOAL_DISTANCE = 0.1; // meters
};

} // namespace ego_planner_adapter

#endif // EGO_PLANNER_ADAPTER__TARGET_RELAY_HPP_